import cv2
import numpy as np
import rect_position
import laser_point
import time
from Datasend import send_data
from points_line_proc import points_rect_judge
from fpsget import calculate_fps
# 初始化变量
laserpoint = None
rect_points = None
outer = None
inner = None
frame_count = 0
start_time = time.time()
# 循环直到成功获取有效的 center_point 和 rect_points
while laserpoint is None or rect_points is None:
    try:
        rect_points,outer,inner = rect_position.rectpoints()
        laserpoint = laser_point.laser_point()
        for i in range(10):
            send_data(rect_points, laserpoint)
        # 检查是否获取到有效值
        if laserpoint is None or rect_points is None:
            print("Failed to get valid values, retrying...")
        else:
            print("Got valid values from rect_position, proceeding to the next code segment.")
    except Exception as e:
        print(f"Error occurred while getting values from rect_position: {e}")
        print("Retry after error...")
        # 重置变量，确保下次循环重新尝试
        laserpoint = None
        rect_points = None
#初始化摄像头
camera = cv2.VideoCapture(0)

#设置摄像头分辨率
camera.set(cv2.CAP_PROP_FRAME_WIDTH, 320)
camera.set(cv2.CAP_PROP_FRAME_HEIGHT, 240)
camera.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc('M', 'J', 'P', 'G'))  # 设置为MJPG格式以提高帧率
camera.set(cv2.CAP_PROP_FPS, 30)
# 检查摄像头是否成功打开
if not camera.isOpened():
    print("无法打开摄像头")
    exit()

while True:
    #读取摄像头帧
    ret, frame = camera.read()
    frame_count += 1
    fps = calculate_fps(start_time, frame_count)

    # 在图像上显示帧率
    cv2.putText(frame, f"FPS: {fps:.2f}", (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2)
    # 在图像上绘制矩形框                                                    
    # cv2.polylines(frame, [outer], isClosed=True, color=(0, 0, 255), thickness=3)
    # cv2.polylines(frame, [inner], isClosed=True, color=(0, 0, 255), thickness=3)
    
    if not ret:
        print("无法从摄像头读取数据")
        break

    # 转换为 HSV 色彩空间
    hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
    
    #定义白色激光的 HSV 范围
    lower_white = np.array([0, 0, 245])
    upper_white = np.array([179, 10, 255])
    mask1 = cv2.inRange(hsv, lower_white, upper_white)    
    # 定义红色激光的 HSV 范围
    lower_red = np.array([160, 100, 200])
    upper_red = np.array([179, 255, 255])
    mask2 = cv2.inRange(hsv, lower_red, upper_red)
    
    cv2.imshow("mask1",mask1)
    cv2.imshow("mask2", mask2)
    # 合并两个红色范围的掩码
    mask = cv2.bitwise_or(mask1, mask2)
    cv2.imshow("mask",mask)

    # 图像增强
    mask = cv2.GaussianBlur(mask, (5, 5), 0)  # 高斯模糊
    mask = cv2.dilate(mask, None, iterations=2)  # 膨胀操作
    kernel = np.ones((5, 5), np.uint8)
    # 开运算
    mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel)
    # 查找轮廓
    _ ,contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
    laser_detected = False
    for contour in contours:
        # 筛选面积较小的轮廓
        if cv2.contourArea(contour) < 10:
            continue

        # 获取轮廓的边界框
        x, y, w, h = cv2.boundingRect(contour)

        # 计算轮廓的中心点
        center_x = x + w // 2
        center_y = y + h // 2
        center_point = (center_x, center_y)
        position=points_rect_judge(center_point, outer, inner)
        if position == 1:
            cv2.putText(frame, "Inside", (230, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0,255,0), 2)
        elif position == 0:
            cv2.putText(frame, "Between", (230, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0,255,0), 2)
        elif position == -1:
            cv2.putText(frame, "Outside", (230, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0,255,0), 2)
        # 在画面上绘制轮廓和中心点
        
        cv2.circle(frame, (center_x, center_y), 5, (0, 0, 255), -1)
        laser_detected = True

    
    cv2.imshow('Frame', frame)
    
    # 退出按键（按 'q' 键退出）
    if cv2.waitKey(1) & 0xFF == ord('q'):
        break

# 释放摄像头资源
camera.release()
# 关闭所有 OpenCV 窗口
cv2.destroyAllWindows()